/*
 * To change this template, choose Tools | Templates
 * and open the template in the editor.
 */
package edu.wpi.first.wpilibj.templates.subsystems;

import edu.wpi.first.wpilibj.command.Subsystem;
import edu.wpi.first.wpilibj.RobotDrive;
import edu.wpi.first.wpilibj.templates.RobotMap;
import edu.wpi.first.wpilibj.templates.commands.DriveWithJoy;
import edu.wpi.first.wpilibj.Gyro;
import edu.wpi.first.wpilibj.ADXL345_I2C;
import edu.wpi.first.wpilibj.Encoder;
import edu.wpi.first.wpilibj.Timer;
import edu.wpi.first.wpilibj.camera.AxisCamera;
import edu.wpi.first.wpilibj.camera.AxisCameraException;
import edu.wpi.first.wpilibj.image.BinaryImage;
import edu.wpi.first.wpilibj.image.ColorImage;
import edu.wpi.first.wpilibj.image.CriteriaCollection;
import edu.wpi.first.wpilibj.image.NIVision.MeasurementType;
import edu.wpi.first.wpilibj.image.NIVisionException;
import edu.wpi.first.wpilibj.image.ParticleAnalysisReport;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;

/**
 *
 * @author Team 3502
 */
public class DriveTrain extends Subsystem {
    // Put methods for controlling this subsystem
    // here. Call these from Commands.
    private RobotDrive drive;
    private Gyro gyro;
    private ADXL345_I2C accel;
    private Encoder encoderLeft;
    private Encoder encoderRight;

    double gyro_o, gyro_n, gyro_filt; //previous gyro readout, new gyro readout, filtered gyro;
    double acc_x, acc_y, acc_z; //accelerometer readouts (x, y, z axes)
    double accel_o, accel_n, accel_filt;
    double tilt, tilt_filt; //a.k.a. angle of inclination on bridge
    //double ky; //accelerometer scaling factor for calculating tilt
    double kx; //accelerometer scaling factor for calculating tilt
    double zo; //twist deadband
    int encLt_count, encRt_count;
    double encLt_dist, encLt_rate, encRt_dist, encRt_rate;


    private double targetx, targety;
    //private double imgctr;

    private AxisCamera camera;          // the axis camera object (connected to the switch)
    private CriteriaCollection cc;      // the criteria for doing the particle filter operation
    //private ParticleAnalysisReport[] reports;

    public void initDefaultCommand() {
        // Set the default command for a subsystem here.
        //setDefaultCommand(new MySpecialCommand());
        setDefaultCommand(new DriveWithJoy());
    }

    public DriveTrain(){
        gyro = new Gyro(RobotMap.gyrochan);
        accel = new ADXL345_I2C (1, ADXL345_I2C.DataFormat_Range.k4G); //can use k2, k4, k8, or k16 -- ex. k8G sets range as +/- 8G
        encoderLeft = new Encoder(RobotMap.encoderLeftA, RobotMap.encoderLeftB);
        encoderRight = new Encoder(RobotMap.encoderRightA, RobotMap.encoderRightB);
        drive = new RobotDrive(RobotMap.frontLeftMotor, RobotMap.rearLeftMotor, RobotMap.frontRightMotor, RobotMap.rearRightMotor);
        drive.setSafetyEnabled(false);
        //ky = 90.0/56.25; //accelerometer y axis scaling factor:  at 90 deg (actual), accelerometer read about 57 degrees (raw)
        kx = 90.0/59.0; //accelerometer z axis scaling factor:
        zo = 0.5;

        camera = AxisCamera.getInstance();  // get an instance of the camera
        cc = new CriteriaCollection();      // create the criteria for the particle filter

        //use different criteria for live shots which are a 480 x 640 image
        cc.addCriteria(MeasurementType.IMAQ_MT_BOUNDING_RECT_WIDTH, 30, 400, false);
        cc.addCriteria(MeasurementType.IMAQ_MT_BOUNDING_RECT_HEIGHT, 40, 400, false);
    }

    public void driveWithJoy(double y, double twist){
        drive.arcadeDrive(-y, -twist);
        //System.out.println("Drive Me");
    }

    //Process images to find the highest target
    public double findtarget(){
        try {
            ColorImage image = camera.getImage(); //gets a snapshot from the camera
            //image.write("/3502getImage.jpg"); //saves the snapshot image to a file
            targety = image.getHeight() + 100;

            //threshold values from lab test setup
            BinaryImage thresholdImage = image.thresholdRGB(0, 134, 150, 255, 94, 255);   // filter out all but green target color

            BinaryImage bigObjectsImage = thresholdImage.removeSmallObjects(false, 2);  // remove small artifacts
            BinaryImage convexHullImage = bigObjectsImage.convexHull(false);          // fill in occluded rectangles
            BinaryImage filteredImage = convexHullImage.particleFilter(cc);           // find filled in rectangles

            ParticleAnalysisReport[] reports = filteredImage.getOrderedParticleAnalysisReports();  //get list of results
            for (int i = 0; i < reports.length; i++) {                                // print results
                ParticleAnalysisReport r = reports[i];
                System.out.print("Particle " + i + ":  Center of mass x = " + r.center_mass_x + ",  ");
                System.out.println("Center of mass y = " + r.center_mass_y);
                //Find the highest hoop
                if (r.center_mass_y < targety){
                    targetx = r.center_mass_x_normalized;
                    targety = r.center_mass_y;
                    System.out.println("Target Particle Number = " + i);
                }
            }
            //System.out.println(filteredImage.getNumberParticles() + "  " + Timer.getFPGATimestamp());

            System.out.println("targetx = " + targetx);
            if (filteredImage.getNumberParticles() == 0){
                System.out.println("NO TARGETS FOUND!!!");
            }

            /**
             * all images in Java must be freed after they are used since they are allocated out
             * of C data structures. Not calling free() will cause the memory to accumulate over
             * each pass of this loop.
             */
            filteredImage.free();
            convexHullImage.free();
            bigObjectsImage.free();
            thresholdImage.free();
            image.free();

        } catch (AxisCameraException ex) {        // this is needed if the camera.getImage() is called
            //System.out.println("AxisCameraException");
            //ex.printStackTrace();
        } catch (NIVisionException ex) {
            //System.out.println("NIVisionException");
            //ex.printStackTrace();
        }
        return targetx;
    }

    public double checktarget(){
        try {
            ColorImage image = camera.getImage(); //gets a snapshot from the camera
            //image = new RGBImage("/1a.JPG"); //use this with test images
            //image.write("/3502getImage.jpg"); //saves the snapshot image to a file

            //use with test images
            //BinaryImage thresholdImage = image.thresholdRGB(0, 255, 0, 255, 150, 255);   // filter out all but green colors

            //numbers from lab test setup
            BinaryImage thresholdImage = image.thresholdRGB(0, 134, 150, 255, 94, 255);   // filter out all but green target color

            BinaryImage bigObjectsImage = thresholdImage.removeSmallObjects(false, 2);  // remove small artifacts
            BinaryImage convexHullImage = bigObjectsImage.convexHull(false);          // fill in occluded rectangles
            BinaryImage filteredImage = convexHullImage.particleFilter(cc);           // find filled in rectangles

            ParticleAnalysisReport[] reports = filteredImage.getOrderedParticleAnalysisReports();  //get list of results

            for (int i = 0; i < reports.length; i++) {                                // print results
                ParticleAnalysisReport r = reports[i];
                System.out.print("Particle " + i + ":  Center of mass x = " + r.center_mass_x + ",  ");
                System.out.println("Center of mass y = " + r.center_mass_y);
                //Make sure that the target stays the same as the original one
                if (r.center_mass_y > (targety-10) && r.center_mass_y < (targety+10)){
                    System.out.println("test... Y found");
                    targetx = r.center_mass_x_normalized;
                }
            }

            System.out.println("targetx = " + targetx);
            if (filteredImage.getNumberParticles() == 0){
                System.out.println("NO TARGETS FOUND!!!");
            }

            /**
             * all images in Java must be freed after they are used since they are allocated out
             * of C data structures. Not calling free() will cause the memory to accumulate over
             * each pass of this loop.
             */
            filteredImage.free();
            convexHullImage.free();
            bigObjectsImage.free();
            thresholdImage.free();
            image.free();

        } catch (AxisCameraException ex) {        // this is needed if the camera.getImage() is called
            //System.out.println("AxisCameraException");
            //ex.printStackTrace();
        } catch (NIVisionException ex) {
            //System.out.println("NIVisionException");
            //ex.printStackTrace();
        }
        return targetx;
    }

    public void initsensors(){
        gyro.reset();
        gyro_o = gyro.getAngle();
        //accel_o = accel.getAcceleration(ADXL345_I2C.Axes.kY)* ky * 180.0 / 3.1416;
        accel_o = accel.getAcceleration(ADXL345_I2C.Axes.kX)* kx * 180.0 / 3.1416;
        encoderLeft.reset();
        encoderLeft.start();
        encoderRight.reset();
        encoderRight.start();
    }

    public void readgyro(){
        gyro_n = gyro.getAngle();
        //System.out.println("Gyro Angle (Raw) = " + gyro_n);
        gyro_filt = (gyro_o - gyro_n) / 2; //high pass filter
        gyro_o = gyro_n;
    }

    public void readaccel(){
        acc_x = accel.getAcceleration(ADXL345_I2C.Axes.kX);
        //System.out.println("Acceleration (X axis)= " + accread);
        //acc_y = accel.getAcceleration(ADXL345_I2C.Axes.kY);
        //System.out.println("Acceleration (Y axis)= " + accread);
        //acc_z = accel.getAcceleration(ADXL345_I2C.Axes.kZ);
        //System.out.println("Acceleration (Z axis)= " + accread);
        //accel_n = acc_y * ky * 180.0 / 3.1416; //convert to degrees
        accel_n = acc_x * kx * 180.0 / 3.1416; //convert to degrees
        accel_filt = (accel_o + accel_n) / 2; //low pass filter
        accel_o = accel_n;
    }

    public void findtilt(){
        //calculate tilt a.k.a. angle of inclination
        //tilt = acc_y * ky * 180.0 / 3.1416;
        tilt_filt = (gyro_filt + accel_filt) - 4.25 + 90.0; //complimentary filter, adjusting for mounting angle, and shifting 90 degrees
    }

    public void readencoders(){
        //encLt_count = encoderLeft.get();
        encLt_dist = encoderLeft.getDistance();
        encLt_rate = encoderLeft.getRate();
        //encRt_count = encoderRight.get();
        encRt_dist = encoderRight.getDistance();
        encRt_rate = encoderRight.getRate();
    }

    public void readsensors(){
        //SmartDashboard.putDouble("Gyro Angle (Raw/Drifting)", gyro_n);
        //SmartDashboard.putDouble("Gyro Angle (Filtered)", gyro_filt);
        //SmartDashboard.putDouble("Accelerometer X in degrees (Raw)", accel_n);
        //SmartDashboard.putDouble("Accelerometer X in degrees (Filtered)", accel_filt);
        //SmartDashboard.putDouble("Tilt Angle X in degrees (Filtered)", tilt_filt);
        //SmartDashboard.putInt("Left Encoder count", encLt_count);
        //SmartDashboard.putDouble("Left Encoder Distance (inches)", encLt_dist);
        //SmartDashboard.putDouble("Left Encoder Rate", encLt_rate);
        //SmartDashboard.putInt("Right Encoder count", encRt_count);
        //SmartDashboard.putDouble("Right Encoder Distance (inches)", encRt_dist);
        //SmartDashboard.putDouble("Right Encoder Rate", encRt_rate);
    }
    
}

